{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "-3NPWeM5nKTz"
      },
      "source": [
        "# Rot2"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "zKQLwRQWvRAW"
      },
      "source": [
        "A `gtsam.Rot2` represents rotation in 2D space. It models a 2D rotation in the Special Orthogonal Group $\\text{SO}(2)$."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "1MUA6xip5fG4"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/geometry/doc/Rot2.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "7a9Dr6c5nHi1"
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "metadata": {
        "id": "-dp28DoR7WsD"
      },
      "outputs": [],
      "source": [
        "from gtsam import Rot2, Point2\n",
        "import numpy as np"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "gZRXZTrJ7mqJ"
      },
      "source": [
        "## Initialization and properties"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "PK-HWTDm7sU4"
      },
      "source": [
        "A `Rot2` can be initialized with no arguments, which yields the identity rotation, or it can be constructed from an angle in radians, degrees, cos-sin form, or the bearing or arctangent of a 2D point. `Rot2` uses radians to communicate angle by default."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 58,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "oIakIOAB9afi",
        "outputId": "c2cb005d-056f-4a4f-b5ff-2226be1ba3e7"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Identities:\n",
            "0.0\n",
            "0.0\n",
            "Radians:\n",
            "1.5707963267948966\n",
            "1.5707963267948966\n",
            "Degrees:\n",
            "1.5707963267948966\n",
            "Cos-Sin:\n",
            "0.5235987755982988\n",
            "Bearing:\n",
            "0.7853981633974483\n",
            "0.7853981633974483\n"
          ]
        }
      ],
      "source": [
        "# The identity rotation has theta = 0.\n",
        "identity = Rot2()\n",
        "print(\"Identities:\")\n",
        "print(identity.theta())\n",
        "print(Rot2.Identity().theta())\n",
        "\n",
        "# The constructor uses radians, so it is identical to Rot2.fromAngle(r).\n",
        "rads = Rot2(np.pi / 2)\n",
        "also_rads = Rot2.fromAngle(np.pi / 2)\n",
        "print(\"Radians:\")\n",
        "print(rads.theta())\n",
        "print(also_rads.theta())\n",
        "\n",
        "# Rot2.fromDegrees(d) constructs from degrees.\n",
        "degs = Rot2.fromDegrees(90)\n",
        "print(\"Degrees:\")\n",
        "print(degs.theta())\n",
        "\n",
        "# Rot2 can also be constructed using cosine and sine values, if you have them lying around.\n",
        "c = np.cos(np.pi / 6)\n",
        "s = np.sin(np.pi / 6)\n",
        "cs = Rot2.fromCosSin(c, s)\n",
        "print(\"Cos-Sin:\")\n",
        "print(cs.theta())\n",
        "\n",
        "# Construct with bearing to point from theta = 0.\n",
        "p = Point2(2, 2)\n",
        "bear = Rot2.relativeBearing(p)\n",
        "print(\"Bearing:\")\n",
        "print(bear.theta())\n",
        "# Or with atan2(y, x), which accomplishes the same thing.\n",
        "atan = Rot2.atan2(p[1], p[0])\n",
        "print(atan.theta())"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "rHovUXbUys5r"
      },
      "source": [
        "The following properties are available from the standard interface:\n",
        "- `theta()` (in radians)\n",
        "- `degrees()`\n",
        "- `c()` (the cosine value, precalculated)\n",
        "- `s()` (the sine value, precalculated)\n",
        "- `matrix()` (the 2x2 rotation matrix:  $\\begin{bmatrix}\n",
        "\\cos\\theta & -\\sin\\theta \\\\\n",
        "\\sin\\theta & \\cos\\theta\n",
        "\\end{bmatrix}$)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 18,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "P5OXTjFu2DeX",
        "outputId": "70848419-c055-44bc-de11-08e8f93fe3bf"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "theta: 2.35619\n",
            "\n",
            "Radians: 2.356194490192345\n",
            "Degrees: 135.0\n",
            "Cosine: -0.7071067811865475\n",
            "Sine: 0.7071067811865476\n",
            "Matrix:\n",
            "[[-0.70710678 -0.70710678]\n",
            " [ 0.70710678 -0.70710678]]\n"
          ]
        }
      ],
      "source": [
        "example_rot = Rot2(3 * np.pi / 4)\n",
        "\n",
        "# The default print statement includes 'theta: ' and a newline at the end.\n",
        "print(example_rot)\n",
        "\n",
        "print(f\"Radians: {example_rot.theta()}\")\n",
        "print(f\"Degrees: {example_rot.degrees()}\")\n",
        "print(f\"Cosine: {example_rot.c()}\")\n",
        "print(f\"Sine: {example_rot.s()}\")\n",
        "print(f\"Matrix:\\n{example_rot.matrix()}\")\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "PpqHUDnl5rTW"
      },
      "source": [
        "## Basic operations"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "sa4qx58n5tG9"
      },
      "source": [
        "For basic use, a `Rot2` can rotate and unrotate a point."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 25,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "yaBKjGn05_-c",
        "outputId": "fb89fe09-b2b8-496d-e835-f379d197a4eb"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Rotated: [-2.82842712e+00  2.22044605e-16]\n",
            "Unrotated: [-2.  2.]\n",
            "Unrotated again: [-2.22044605e-16  2.82842712e+00]\n"
          ]
        }
      ],
      "source": [
        "rot = Rot2.fromDegrees(45)\n",
        "p = Point2(-2, 2)\n",
        "\n",
        "# Rotate the point at (-2, 2) 45 degrees to the -x axis.\n",
        "rotated = rot.rotate(p)\n",
        "print(f\"Rotated: {rotated}\")\n",
        "# Perform the inverse rotation with unrotate()\n",
        "print(f\"Unrotated: {rot.unrotate(rotated)}\")\n",
        "# Of course, unrotating a point you didn't rotate just rotates it backwards.\n",
        "print(f\"Unrotated again: {rot.unrotate(p)}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "RVFCoBpW6Bvh"
      },
      "source": [
        "Also, the `equals()` function allows for comparison of two `Rot2` objects with a tolerance."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 31,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "m74YbK5h6CPU",
        "outputId": "1b16695e-cdfe-4348-875f-c41d8b4a3b26"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "True\n",
            "False\n"
          ]
        }
      ],
      "source": [
        "eq_rads = Rot2(np.pi / 4)\n",
        "eq_degs = Rot2.fromDegrees(45)\n",
        "\n",
        "print(eq_rads.equals(eq_degs, 1e-8))\n",
        "\n",
        "# Direct comparison does not work for Rot2.\n",
        "print(eq_rads == eq_degs)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "ko9KSZgd4bCp"
      },
      "source": [
        "## Lie group $\\text{SO}(2)$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "76D2KkX241zX"
      },
      "source": [
        "### Group operations\n",
        "\n",
        "`Rot2` implements the group operations `inverse`, `compose`, `between` and `identity`. For more information on groups and their use here, see [GTSAM concepts](https://gtsam.org/notes/GTSAM-Concepts.html)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 57,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "QZ_NTeXK87Wq",
        "outputId": "12af920d-8f86-473d-88ab-ee57d316cb72"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Inverse:\n",
            "theta: -0.523599\n",
            "\n",
            "Compose:\n",
            "theta: 1.5708\n",
            "\n",
            "theta: 1.5708\n",
            "\n",
            "Between:\n",
            "theta: 0.523599\n",
            "\n",
            "Identity:\n",
            "theta: 0\n",
            "\n"
          ]
        }
      ],
      "source": [
        "a = Rot2(np.pi / 6)\n",
        "b = Rot2(np.pi / 3)\n",
        "\n",
        "# The inverse of a Rot2 is just the negative of its angle.\n",
        "print(\"Inverse:\")\n",
        "print(a.inverse())\n",
        "\n",
        "# The composition of two Rot2 objects is their angles added together.\n",
        "# The operator for compose is *, but make no mistake, this does not multiply the angles.\n",
        "print(\"Compose:\")\n",
        "print(a * b)\n",
        "print(a.compose(b))\n",
        "\n",
        "# Between gives the difference between the two angles.\n",
        "print(\"Between:\")\n",
        "print(a.between(b))\n",
        "\n",
        "# The identity is theta = 0, as above.\n",
        "print(\"Identity:\")\n",
        "print(Rot2.Identity())"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "YfiYuVpL-cgq"
      },
      "source": [
        "## Lie group operations\n",
        "\n",
        "`Rot2` implements the Lie group operations for exponential mapping and log mapping. For more information on Lie groups and their use here, see [GTSAM concepts](https://gtsam.org/notes/GTSAM-Concepts.html)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 54,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "4JiDEGqG-van",
        "outputId": "56047f8d-3349-4ee6-e73c-cd2fa1efb95d"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "theta: 1.5708\n",
            "\n",
            "theta: 3.14159\n",
            "\n",
            "[1.57079633]\n",
            "[-0.78539816]\n",
            "[-0.78539816]\n"
          ]
        }
      ],
      "source": [
        "r = Rot2(np.pi / 2)\n",
        "w = Rot2(np.pi / 4)\n",
        "v = [np.pi / 2]\n",
        "\n",
        "# The exponential map transforms a 1-dimensional vector representing an angle\n",
        "# into its Rot2 equivalent.\n",
        "print(Rot2.Expmap(v))\n",
        "# The retract function takes the exponential map of the supplied 1D vector and\n",
        "# composes it with the calling Rot2.\n",
        "print(r.retract(v))\n",
        "\n",
        "# The static log map transforms a Rot2 into its 1D vector equivalent.\n",
        "print(Rot2.Logmap(r))\n",
        "# The member log map transforms a Rot2 into its 1D vector equivalent relative to\n",
        "# the Rot2 calling the function.\n",
        "print(r.logmap(w))\n",
        "# logmap is the same as calculating the coordinate of the second Rot2 in the\n",
        "# local frame of the first, which localCoordinates (inherited from LieGroup) does.\n",
        "print(r.localCoordinates(w))\n"
      ]
    }
  ],
  "metadata": {
    "colab": {
      "provenance": []
    },
    "kernelspec": {
      "display_name": "Python 3",
      "name": "python3"
    },
    "language_info": {
      "name": "python"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}
